const int BUFFER_SIZE = 9;

//const int PINS[9] = {7, 8, 9, 10, 11, 12, 13, 14, 15};  // 0=IF 1=IK 2=MF 3=MK 4=RF 5=RK 6=PF 7=PK 8=T
const int PINS[9] = {0, 1, 2, 3, 4, 5, 4, 5, 3};
int buffer[9][BUFFER_SIZE];
int buffer_pos[9] = {0};

int flat[9][BUFFER_SIZE];
int flat_pos[9] = {0};
int bent[9][BUFFER_SIZE];
int bent_pos[9] = {0};

//array position (0 = flat, 1 = bent) for that sensor
int thumb[2];
int indexFinger[2];
int indexKnuck[2];
int middleFinger[2];
int middleKnuck[2];
int ringFinger[2];
int ringKnuck[2];
int pinkyFinger[2];
int pinkyKnuck[2];

float valuesPerDeg[9];  // 0=IF 1=IK 2=MF 3=MK 4=RF 5=RK 6=PF 7=PK 8=T

int calibrationDelay = 1000;
char sSerialString[512];


void setup()
{
 Serial.begin(9600); 
 
 digitalWrite(7, HIGH);
 digitalWrite(8, LOW);
 digitalWrite(9, LOW);
 
 Serial.print("Starting");
 delay(calibrationDelay);
 Serial.print("1");
 delay(calibrationDelay);
 Serial.print("2");
 delay(calibrationDelay);
 Serial.print("3");
 delay(calibrationDelay);
 
 //turn red led on
 int a;
 for(a=0; a<9; a++)
 {
   
   indexFinger[0] = getFlatValues(0);
   indexKnuck[0] = getFlatValues(1);
   middleFinger[0] = getFlatValues(2);
   middleKnuck[0] = getFlatValues(3);
   ringFinger[0] = getFlatValues(4);
   ringKnuck[0] = getFlatValues(5);
   pinkyFinger[0] = getFlatValues(6);
   pinkyKnuck[0] = getFlatValues(7);
   thumb[0] = getFlatValues(8);
   
 }
 
 digitalWrite(7, LOW);
 digitalWrite(8, HIGH);
 
 Serial.print("Bend um bitch");
 delay(calibrationDelay);
 Serial.print("1");
 delay(calibrationDelay);
 Serial.print("2");
 delay(calibrationDelay);
 Serial.print("3");
 delay(calibrationDelay);
 
 //blink red led twice
 
 

 for(a=0; a<9; a++)
 {
   
   indexFinger[1] = getFlatValues(0);
   indexKnuck[1] = getFlatValues(1);
   middleFinger[1] = getFlatValues(2);
   middleKnuck[1] = getFlatValues(3);
   ringFinger[1] = getFlatValues(4);
   ringKnuck[1] = getFlatValues(5);
   pinkyFinger[1] = getFlatValues(6);
   pinkyKnuck[1] = getFlatValues(7);
   thumb[1] = getFlatValues(8);
 }
 
 
 valuesPerDeg[0] = dataPtsPerDeg(indexFinger);
 valuesPerDeg[1] = dataPtsPerDeg(indexKnuck);
 valuesPerDeg[2] = dataPtsPerDeg(middleFinger);
 valuesPerDeg[3] = dataPtsPerDeg(middleKnuck);
 valuesPerDeg[4] = dataPtsPerDeg(ringFinger);
 valuesPerDeg[5] = dataPtsPerDeg(ringKnuck);
 valuesPerDeg[6] = dataPtsPerDeg(pinkyFinger);
 valuesPerDeg[7] = dataPtsPerDeg(pinkyKnuck);
 valuesPerDeg[8] = dataPtsPerDeg(thumb);
 
 //turn off red led turn on green led
 digitalWrite(8, LOW);
 digitalWrite(9, HIGH);
  
 Serial.print("Thumb Flat: ");
 Serial.print(thumb[0]);
 Serial.print("  Thumb Bent: ");
 Serial.print(thumb[1]);
 
 Serial.print("  Index Finger Flat: ");
 Serial.print(indexFinger[0]);
 Serial.print("  Index Finger Bent: ");
 Serial.print(indexFinger[1]);
 
 Serial.print("  Index Knuck Flat: ");
 Serial.print(indexKnuck[0]);
 Serial.print("  Index Knuck Bent: ");
 Serial.print(indexKnuck[1]);
 
 Serial.print("  Middle Finger Flat: ");
 Serial.println(middleFinger[0]);
 Serial.print("  Middle Finger Bent : ");
 Serial.print(middleFinger[1]);
 
 Serial.print("  Middle Knuck Flat: ");
 Serial.print(middleKnuck[0]);
 Serial.print("  Middle Knuck Bent: ");
 Serial.print(middleKnuck[1]);
 
 Serial.print("  Pinky Finger Flat: ");
 Serial.println(pinkyFinger[0]);
 Serial.print("  Pinky Finger Bent: ");
 Serial.println(pinkyFinger[1]); 
 
 Serial.print("Values per deg IF = ");
 Serial.print(valuesPerDeg[0]);
 Serial.print("  IK = ");
 Serial.println(valuesPerDeg[1]);
 
}

void loop()
{
  
 int indexAngleF = get_angle(indexFinger, 0, valuesPerDeg[0]);
 int indexAngleK = get_angle(indexKnuck, 1, valuesPerDeg[1]) ;
 int middleAngleF = get_angle(middleFinger, 2, valuesPerDeg[2]);
 int middleAngleK = get_angle(middleKnuck, 3, valuesPerDeg[3]);
 int ringAngleF = get_angle(ringFinger, 4, valuesPerDeg[4]);
 int ringAngleK = get_angle(ringKnuck, 5, valuesPerDeg[5]);
 int pinkyAngleF = get_angle(pinkyFinger, 6, valuesPerDeg[6]);
 int pinkyAngleK = get_angle(pinkyKnuck, 7, valuesPerDeg[7]);
 int thumbAngle = get_angle(thumb, 8, valuesPerDeg[8]);

 Serial.print("IF: ");
 Serial.print(indexAngleF);
 Serial.print("  IK: ");
 Serial.print(indexAngleK);
 Serial.print("  MF: ");
 Serial.print(middleAngleF);
 Serial.print("  MK: ");
 Serial.print(middleAngleK);
 Serial.print("  RF: ");
 Serial.print(ringAngleF);
 Serial.print("  RK: ");
 Serial.println(ringAngleK);
 Serial.print("  PF: ");
 Serial.print(pinkyAngleF);
 Serial.print("  PK: ");
 Serial.print(pinkyAngleK);
 Serial.print("  T: ");
Serial.println(thumbAngle);
  
}


int get_Value(const int pinNum)
{
  delay(1);
  buffer[pinNum][buffer_pos[pinNum]] = analogRead(PINS[pinNum]);
  buffer_pos[pinNum] = (buffer_pos[pinNum]+1) % BUFFER_SIZE;
  
  long sum = 0;
  for(int i=0; i<BUFFER_SIZE; i++)
    sum += buffer[pinNum][i];
    
  return (round(sum/BUFFER_SIZE));
}

int getFlatValues(const int pinNum)
{
  delay(1);
  flat[pinNum][flat_pos[pinNum]] = analogRead(PINS[pinNum]);
  flat_pos[pinNum] = (flat_pos[pinNum]+1) % BUFFER_SIZE;
  
  long sum = 0;
  for(int i=0; i<BUFFER_SIZE; i++)
    sum += flat[pinNum][i];
    
  return (round(sum/BUFFER_SIZE));
}

int getBentValues(const int pinNum)
{
   delay(1);
  bent[pinNum][bent_pos[pinNum]] = analogRead(PINS[pinNum]);
  bent_pos[pinNum] = (bent_pos[pinNum]+1) % BUFFER_SIZE;
  
  long sum = 0;
  for(int i=0; i<BUFFER_SIZE; i++)
    sum += bent[pinNum][i];
    
  return (round(sum/BUFFER_SIZE));
}

float dataPtsPerDeg(int array[])
{
  float tmp = array[1]-array[0];
  
  return tmp/90; 
}

int get_angle(int array[], const int pinNum, float valuesPerDeg)
{

  int currentVal = get_Value(pinNum);

  int angle = round((currentVal-array[0])/valuesPerDeg);
  
  return angle;
}
